/**
 * @file SRPQ_Controller.h
 * @author Jozenlee (2250007028@qq.com)
 * @brief 
 * @version 0.1
 * @date 2022-03-09
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#ifndef _SRPQ_CONTROLLER_H_
#define _SRPQ_CONTROLLER_H_
#include "robot/RobotController.h"
#include "common/Controllers/GaitScheduler.h"
#include "common/ControlParameters/ControlParameters.h"
#include "srpq_controller/FSM_States/ControlFSM.h"

class SRPQ_Controller: public RobotController{
public:
  SRPQ_Controller();
  virtual ~SRPQ_Controller(){}

  virtual void initializeController();
  virtual void runController();
  virtual void updateVisualization(){}
  virtual void Estop(){ 
    //   _controlFSM->initialize(); 
    }

protected:
  ControlFSM<float>* _controlFSM;
  
  // Gait Scheduler controls the nominal contact schedule for the feet
  GaitScheduler<float>* _gaitScheduler;

private:
    UserParameters userParameters;
};

#endif